#pragma once
#include <array>
#include <cstddef>
#include <initializer_list>
#include "matrixExpr.h"
#include <stdint.h>
#include <stdexcept>
#define DEG2RAD(theta) (0.01745329251994329 * (theta))
namespace math
{
    template <typename T, std::size_t Rows, std::size_t Cols>
    class Matrix : public MatrixExpr<Matrix<T, Rows, Cols>, T, Rows, Cols>
    {
    public:
        using value_type = T;
        using array_type = std::array<T, Cols>;

        // 数据存储（行主序）
        std::array<array_type, Rows> data;

        // 构造函数（初始化所有元素为0）
        Matrix() : data{} {}

        // 构造函数
        Matrix(std::initializer_list<std::initializer_list<T>> init)
        {
            if (init.size() != Rows)
                throw std::invalid_argument("Row count mismatch");

            auto row_iter = init.begin();
            for (std::size_t i = 0; i < Rows; ++i, ++row_iter)
            {
                if (row_iter->size() != Cols)
                    throw std::invalid_argument("Column count mismatch in row " + std::to_string(i));

                auto col_iter = row_iter->begin();
                for (std::size_t j = 0; j < Cols; ++j, ++col_iter)
                {
                    data[i][j] = *col_iter;
                }
            }
        }

        // 元素访问（行优先）
        T &operator()(std::size_t row, std::size_t col)
        {
            return data[row][col];
        }
        const T &operator()(std::size_t row, std::size_t col) const
        {
            return data[row][col];
        }

        auto transpose() const
        {
            return TransposeExpr<Matrix, T, Rows, Cols>(*this);
        }

        void printf()
        {
            for (int i = 0; i < Rows; i++)
            {
                for (int j = 0; j < Cols; j++)
                {
                    std::cout << data[i][j] << ",";
                }
                std::cout << std::endl;
            }
        }

        Matrix inverse() const
        {
            static_assert(Rows <= 4, "Only implemented for N<=4");
            static_assert(Rows == Cols, "Only implemented Rows == Cols");
            if constexpr (Rows == 3)
                return inverse_3x3();
            if constexpr (Rows == 4)
                return inverse_4x4();
        }
        template <typename Expr>
        Matrix &operator=(const MatrixExpr<Expr, T, Rows, Cols> &expr)
        {

            for (std::size_t i = 0; i < Rows; ++i)
            {
                for (std::size_t j = 0; j < Cols; ++j)
                {

                    data[i][j] = expr(i, j);
                }
            }
            return *this;
        }

        Matrix inverse_3x3() const
        {
            T det = determinant_3x3();
            if (std::abs(det) < 1e-10)
            {
                throw std::runtime_error("Matrix is singular and cannot be inverted.");
            }

            Matrix<T, 3, 3> adjugate;
            for (size_t i = 0; i < 3; ++i)
            {
                for (size_t j = 0; j < 3; ++j)
                {
                    adjugate(j, i) = cofactor_3X3(i, j) / det;
                }
            }
            return adjugate;
        }

        // 计算行列式
        T determinant_3x3() const
        {
            return data[0][0] * cofactor_3X3(0, 0) + data[0][1] * cofactor_3X3(0, 1) + data[0][2] * cofactor_3X3(0, 2);
        }

        T cofactor_3X3(size_t row, size_t col) const
        {
            int sign = ((row + col) % 2 == 0) ? 1 : -1;
            return sign * minor_3X3(row, col);
        }

        // 计算余子式
        T minor_3X3(size_t row, size_t col) const
        {
            std::array<std::array<T, 2>, 2> submatrix;
            size_t sub_i = 0, sub_j = 0;
            for (size_t i = 0; i < 3; ++i)
            {
                if (i == row)
                    continue;
                sub_j = 0;
                for (size_t j = 0; j < 3; ++j)
                {
                    if (j == col)
                        continue;
                    submatrix[sub_i][sub_j++] = data[i][j];
                }
                ++sub_i;
            }
            return submatrix[0][0] * submatrix[1][1] - submatrix[0][1] * submatrix[1][0];
        }

        Matrix inverse_4x4() const
        {
            T det = determinant_4x4();
            if (std::abs(det) < 1e-10)
            {
                throw std::runtime_error("Matrix is singular and cannot be inverted.");
            }

            Matrix adjugate;
            for (size_t i = 0; i < 4; ++i)
            {
                for (size_t j = 0; j < 4; ++j)
                {
                    adjugate(j, i) = cofactor_4x4(i, j) / det;
                }
            }
            return adjugate;
        }

        // 计算行列式
        T determinant_4x4() const
        {
            return data[0][0] * cofactor_4x4(0, 0) + data[0][1] * cofactor_4x4(0, 1) + data[0][2] * cofactor_4x4(0, 2) + data[0][3] * cofactor_4x4(0, 3);
        }

        // 计算代数余子式
        T cofactor_4x4(size_t row, size_t col) const
        {
            int sign = ((row + col) % 2 == 0) ? 1 : -1;
            return sign * minor_4x4(row, col);
        }

        // 计算余子式
        T minor_4x4(size_t row, size_t col) const
        {
            std::array<std::array<T, 3>, 3> submatrix;
            size_t sub_i = 0, sub_j = 0;
            for (size_t i = 0; i < 4; ++i)
            {
                if (i == row)
                    continue;
                sub_j = 0;
                for (size_t j = 0; j < 4; ++j)
                {
                    if (j == col)
                        continue;
                    submatrix[sub_i][sub_j++] = data[i][j];
                }
                ++sub_i;
            }
            Matrix<T, 3, 3> submatrix_3x3 = {
                {submatrix[0][0], submatrix[0][1], submatrix[0][2]},
                {submatrix[1][0], submatrix[1][1], submatrix[1][2]},
                {submatrix[2][0], submatrix[2][1], submatrix[2][2]}};
            return submatrix_3x3.determinant_3x3();
        }

        Vector<T,4> operator*(const  Vector<T,4> &v) const
        {
             Vector<T,4> result;
            for (int i = 0; i < 4; ++i)
            {
                result[i] =
                    data[i][0] * v[0] +
                    data[i][1] * v[1] +
                    data[i][2] * v[2] +
                    data[i][3] * v[3];
            }
            return result;
        }
    };
    using mat3f = Matrix<float, 4, 4>;
    using mat4f = Matrix<float, 4, 4>;
}